/**
 * @file MainTask.c
 * @author SoBigDog (623644973lubin@163.com)
 * @brief 
 * @version 0.1
 * @date 2020-11-10
 * 
 * @copyright Copyright (c) 2020
 * 
 */
#include "MainTask.h"
#include "Joystick.h"
#include "ShankConfig.h"

#include "NewProtocolCurtisSeer.h"

extern CAN_HandleTypeDef hcan;
extern struct ShankConfig shank_config;
const struct driver_protocol * walk_motor_protocol;

static void MX_CAN_Init(void)
{
	hcan.Instance = CAN1;

	if (shank_config.can_baudrate == 50000) {
		hcan.Init.Prescaler = 60;
	} else if (shank_config.can_baudrate == 100000) {
		hcan.Init.Prescaler = 30;
	} else if (shank_config.can_baudrate == 125000) {
		hcan.Init.Prescaler = 24;
	} else if (shank_config.can_baudrate == 250000) {
		hcan.Init.Prescaler = 12;
	} else if (shank_config.can_baudrate == 500000) {
		hcan.Init.Prescaler = 6;
	} else if (shank_config.can_baudrate == 1000000) {
		hcan.Init.Prescaler = 3;
	}
	
	hcan.Init.Mode = CAN_MODE_NORMAL;
	hcan.Init.SyncJumpWidth = CAN_SJW_1TQ;
	hcan.Init.TimeSeg1 = CAN_BS1_8TQ;
	hcan.Init.TimeSeg2 = CAN_BS2_3TQ;
	hcan.Init.TimeTriggeredMode = ENABLE;
	hcan.Init.AutoBusOff = ENABLE;
	hcan.Init.AutoWakeUp = DISABLE;
	hcan.Init.AutoRetransmission = ENABLE;
	hcan.Init.ReceiveFifoLocked = ENABLE;
	hcan.Init.TransmitFifoPriority = ENABLE;
	if (HAL_CAN_Init(&hcan) != HAL_OK) {
		Error_Handler();
	}

	if (1 == shank_config.can_res) {
		HAL_GPIO_WritePin(CAN_RES_SEL_GPIO_Port, CAN_RES_SEL_Pin, GPIO_PIN_RESET);
	} else {
		HAL_GPIO_WritePin(CAN_RES_SEL_GPIO_Port, CAN_RES_SEL_Pin, GPIO_PIN_SET);
	}
}


static void TaskInit(void) 
{
	MX_CAN_Init();
	HAL_CAN_Start(&hcan);
	
	walk_motor_protocol = &ProtocolCurtisSeer;
}

struct Joystick walk_joy = {
	.u8ChannelX = 4,
	.u8ChannelY = 5,
};

struct Joystick fork_left_joy = {
	.u8ChannelX = 2,
	.u8ChannelY = 3,
};

struct Joystick fork_right_joy = {
	.u8ChannelX = 0,
	.u8ChannelY = 1,
};

void MainTask(void const * arg)
{
	float temp_value;

	TaskInit();

	JoystickInit(&walk_joy);
	JoystickInit(&fork_left_joy);
	JoystickInit(&fork_right_joy);

	while (1) {
		vTaskDelay(30);

		JoystickUpdate(&walk_joy);
		
		temp_value = walk_joy.Vector.value;

		if (walk_joy.Vector.angle < 0) {
			temp_value = -temp_value;
			walk_joy.Vector.angle += PI;
		}
		
		walk_motor_protocol->encode(DRV_CMD_TARGET_SPEED,
					temp_value * 1500 / 2048);

		walk_motor_protocol->encode(DRV_CMD_TARGET_POS,
					(walk_joy.Vector.angle - PI / 2) * 1800 / PI);
	}
}

